/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "arm_math.h"

#include "tim.h"
#include "adc.h"
#include "opamp.h"
#include "usart.h"

#include "bsp.h"
#include "motor.h"
#include "button.h"
#include "lowpass_filter.h"


/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */



/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
#define CH_COUNT 8
struct Frame
{
	float fdata[CH_COUNT];
	unsigned char tail[4];
};

struct Frame vfoaFrame = {
	.tail = {0x00, 0x00, 0x80, 0x7f}
};

float idRef = 0.0f;
float iqRef = 0.1f;
float theta = 0.01f;
bool uartIdle;
uint32_t vofaCmdLen = 0;
uint8_t vofaRxItBuf[1];
#define UART3_BUF_MAX_SIZE   4096
uint8_t uart3Buf[UART3_BUF_MAX_SIZE];
uint8_t vofaCmdBuf[1024];


/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
	.name = "defaultTask",
	.priority = (osPriority_t) osPriorityLow,
	.stack_size = 256 * 4
};
/* Definitions for gBtnScanTask */
osThreadId_t gBtnScanTaskHandle;
const osThreadAttr_t gBtnScanTask_attributes = {
	.name = "gBtnScanTask",
	.priority = (osPriority_t) osPriorityNormal,
	.stack_size = 256 * 4
};
/* Definitions for gGetAngleTask */
osThreadId_t gGetAngleTaskHandle;
const osThreadAttr_t gGetAngleTask_attributes = {
	.name = "gGetAngleTask",
	.priority = (osPriority_t) osPriorityNormal,
	.stack_size = 256 * 4
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

static void bsp_btn_event_callback(void *btn, int event);

/**
 * @brief Control motor work status
 * @param btn
 * @param event
 */
static void bsp_btn_event_callback(void *btn, int event)
{
	if (btn == &bsp_btn1)
	{
	}
	else if (btn == &bsp_btn2)
	{
	}
	else if (btn == &bsp_btn3)
	{
		if (event == ButtonEvent_SingleClick)
		{
			motor1.status = motor1.status == MOTOR_STATUS_RUN ? MOTOR_STATUS_STOP : MOTOR_STATUS_RUN;

			if (motor1.status == MOTOR_STATUS_RUN)
			{
				motor_enable(&motor1);
			}
			else
			{
				motor_disable(&motor1);
			}
		}
	}
	else
	{
	}

	HAL_GPIO_TogglePin(LED_STATUS_GPIO_Port, LED_STATUS_Pin);
}

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void StartBtnScanTask(void *argument);

void StartGetAngleTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void)
{
	/* USER CODE BEGIN Init */
	//printf("Entry MX_FREERTOS_Init\n");

	bsp_init();

	button_install_event_callback(&bsp_btn1, ButtonEvent_SingleClick, bsp_btn_event_callback);
	button_install_event_callback(&bsp_btn2, ButtonEvent_SingleClick, bsp_btn_event_callback);
	button_install_event_callback(&bsp_btn3, ButtonEvent_SingleClick, bsp_btn_event_callback);


	/* Calibrate encoder electricity zero position */
//    motor_align_encoder(&motor1);

//    motor1.dqVoltage.uq = 0.5f;
//    motor1.dqVoltage.ud = 0.0f;

	/* vofa debug */
	HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);

//    HAL_ADCEx

	/* USER CODE END Init */

	/* USER CODE BEGIN RTOS_MUTEX */
	/* add mutexes, ... */
	/* USER CODE END RTOS_MUTEX */

	/* USER CODE BEGIN RTOS_SEMAPHORES */
	/* add semaphores, ... */
	/* USER CODE END RTOS_SEMAPHORES */

	/* USER CODE BEGIN RTOS_TIMERS */
	/* start timers, add new ones, ... */
	/* USER CODE END RTOS_TIMERS */

	/* USER CODE BEGIN RTOS_QUEUES */
	/* add queues, ... */
	/* USER CODE END RTOS_QUEUES */

	/* Create the thread(s) */
	/* creation of defaultTask */
	defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

	/* creation of gBtnScanTask */
	gBtnScanTaskHandle = osThreadNew(StartBtnScanTask, NULL, &gBtnScanTask_attributes);

	/* creation of gGetAngleTask */
	gGetAngleTaskHandle = osThreadNew(StartGetAngleTask, NULL, &gGetAngleTask_attributes);

	/* USER CODE BEGIN RTOS_THREADS */
	/* add threads, ... */
	/* USER CODE END RTOS_THREADS */

	/* USER CODE BEGIN RTOS_EVENTS */
	/* add events, ... */
	/* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
	/* USER CODE BEGIN StartDefaultTask */
	const float udcGain = 3.3f / 4096 * 26;

	/* Infinite loop */
	for (;;)
	{
		HAL_GPIO_TogglePin(LED_BLINK_GPIO_Port, LED_BLINK_Pin);

		HAL_ADC_Start(&hadc2);
		motor1.udc = (float32_t) HAL_ADC_GetValue(&hadc2) * udcGain;
		HAL_Delay(10);

		osDelay(1);
	}
	/* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartBtnScanTask */

/**
* @brief Function implementing the gBtnScanTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartBtnScanTask */
void StartBtnScanTask(void *argument)
{
	/* USER CODE BEGIN StartBtnScanTask */

	/* Infinite loop */
	for (;;)
	{
		bsp_btn_ticks();
		HAL_Delay(5);

		osDelay(1);
	}
	/* USER CODE END StartBtnScanTask */
}

/* USER CODE BEGIN Header_StartGetAngleTask */
/**
* @brief Function implementing the gGetAngleTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartGetAngleTask */
void StartGetAngleTask(void *argument)
{
	/* USER CODE BEGIN StartGetAngleTask */
	static uint32_t lastTick;
	uint32_t tick = 0;
	uint32_t detTick = 0;

	/* Infinite loop */
	for (;;)
	{
//		tick = bsp_get_us_tick();
//
//		/* Set lastTick default value. */
//		static bool firstHere = true;
//		if (firstHere)
//		{
//			motor1.lastTick = tick;
//			firstHere = false;
//			continue;
//		}
//
//		/* 计算间隔时间 */
//		if (tick < motor1.lastTick)
//			detTick = htim16.Instance->ARR - motor1.lastTick + tick;
//		else
//			detTick = tick - motor1.lastTick;
//
//		/* 记录此刻时间 */
//		motor1.lastTick = tick;
//
//		//vfoaFrame.fdata[2] = low_pass_filter_work(&motor1.speedLdFilter, motor_get_speed_rpm(&motor1));
//		vfoaFrame.fdata[7] = (float)(detTick);

		HAL_Delay(1);

		osDelay(1);
	}
	/* USER CODE END StartGetAngleTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
	/* Calc ADC offset */
	static bool calcAdcOffsetOvered = false;
	static uint32_t iaOffset = 0, ibOffset = 0, icOffset = 0;
	const int measCnt = 100;
	static int measCntCopy = measCnt;
	if (hadc->Instance == ADC1 && !calcAdcOffsetOvered)
	{
		iaOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
		icOffset += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
//        ibOffset += HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);

		if (--measCntCopy <= 0)
		{
			bsp_board.iaOffset = iaOffset / measCnt;
			bsp_board.icOffset = icOffset / measCnt;
//            bsp_board.ibOffset = ibOffset / measCnt;

			calcAdcOffsetOvered = true;
		}
	}

	/* foc algorithm */
	if (hadc->Instance == ADC1 && calcAdcOffsetOvered)
	{
		/* Get phase current */
		bsp_get_phase_current(&bsp_board, &motor1.phaseCurrent.ia, &motor1.phaseCurrent.ib, &motor1.phaseCurrent.ic);

		/* Get motor electricity angle */
		motor_get_elec_angle(&motor1);

//	    motor_open_loop_test(&motor1, 2.0f);

		if (motor1.status == MOTOR_STATUS_RUN)
		{
			motor_current_closed_loop(&motor1, idRef, iqRef);
		}

		/* vofa debug */
		vfoaFrame.fdata[0] = motor1.phaseCurrent.ia;
		vfoaFrame.fdata[1] = motor1.phaseCurrent.ib;
//	    vfoaFrame.fdata[2] = motor1.theta;
	    vfoaFrame.fdata[2] = low_pass_filter_work(&motor1.speedLdFilter, motor_get_speed_rpm(&motor1));

//        vfoaFrame.fdata[3] = motor1.dCurrentPid.kp;
//        vfoaFrame.fdata[4] = motor1.dCurrentPid.ki;
//        vfoaFrame.fdata[5] = motor1.dqCurrent.id;
//        vfoaFrame.fdata[6] = idRef;

		vfoaFrame.fdata[3] = motor1.qCurrentPid.kp;
		vfoaFrame.fdata[4] = motor1.qCurrentPid.ki;
		vfoaFrame.fdata[5] = motor1.dqCurrent.iq;
		vfoaFrame.fdata[6] = iqRef;

		vfoaFrame.fdata[3] = (float)motor1.pwmParam.ccra;
		vfoaFrame.fdata[4] = (float)motor1.pwmParam.ccrb;
		vfoaFrame.fdata[5] = (float)motor1.pwmParam.ccrc;

		vfoaFrame.fdata[7] = motor1.dqVoltage.uq;
//		vfoaFrame.fdata[7] = __HAL_TIM_GET_COUNTER(&htim16);
//		vfoaFrame.fdata[7] = motor1.theta;


		HAL_UART_Transmit_DMA(&huart3, (uint8_t *) (&vfoaFrame), sizeof(vfoaFrame));
	}
}


/**
 * @brief Parse vofa cmd
 * @param cmdBuf
 * @return
 */
static float vofa_cmd_parse(uint8_t *cmdBuf, char *arg)
{
	return atof(cmdBuf + strlen(arg));
}

/**
  * @brief  Reception Event Callback (Rx event notification called after use of advanced reception service).
  * @param  huart UART handle
  * @param  Size  Number of data available in application reception buffer (indicates a position in
  *               reception buffer until which, data are available)
  * @retval None
  */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
	if (huart->Instance == USART3)
	{
		if (strstr(vofaCmdBuf, "uq"))
		{
			motor1.dqVoltage.uq = vofa_cmd_parse(vofaCmdBuf, "uq");
		}
		else if (strstr(vofaCmdBuf, "idkp="))
		{
			float kp = vofa_cmd_parse(vofaCmdBuf, "idkp=");
			motor1.dCurrentPid.kp = kp;
		}
		else if (strstr(vofaCmdBuf, "idki="))
		{
			float ki = vofa_cmd_parse(vofaCmdBuf, "idkp=");
			motor1.dCurrentPid.ki = ki;
		}
		else if (strstr(vofaCmdBuf, "iqkp="))
		{
			float kp = vofa_cmd_parse(vofaCmdBuf, "iqkp=");
			motor1.qCurrentPid.kp = kp;
		}
		else if (strstr(vofaCmdBuf, "iqki="))
		{
			float ki = vofa_cmd_parse(vofaCmdBuf, "iqki=");
			motor1.qCurrentPid.ki = ki;
		}
		else if (strstr(vofaCmdBuf, "iqref="))
		{
			float iqref = vofa_cmd_parse(vofaCmdBuf, "iqref=");
			iqRef = iqref;
		}
		else
		{
		}

		memset(vofaCmdBuf, 0, 1024);
		HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);
	}
}



/* USER CODE END Application */

